#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
图形界面的PoseStamped消息发布器
功能：通过滑条控制发布geometry_msgs::PoseStamped消息到自定义话题
创建时间：2025年7月4日
"""

import sys
import rospy
import tkinter as tk
from tkinter import ttk, messagebox
from geometry_msgs.msg import PoseStamped, Point, Quaternion
from tf.transformations import quaternion_from_euler
import threading
import time

# default_topic_name = "/vision_target" 
default_topic_name = "/control/position_cmd"

class PosePublisherGUI:
    def __init__(self, root):
        self.root = root
        self.root.title("PoseStamped消息发布器")
        self.root.geometry("800x700")
        self.root.resizable(True, True)
        
        # ROS初始化
        try:
            rospy.init_node('pose_publisher_gui', anonymous=True)
        except rospy.exceptions.ROSException:
            messagebox.showerror("错误", "ROS节点初始化失败！请确保roscore正在运行。")
            sys.exit(1)
        
        # 发布器变量
        self.publisher = None
        self.topic_name = default_topic_name # 默认话题名
        self.publish_rate = 10.0  # 发布频率Hz
        self.is_publishing = False
        self.publish_thread = None
        
        # 位置和姿态变量
        self.position = {'x': 0.0, 'y': 0.0, 'z': 0.0}  # z轴初值改为0
        self.orientation = {'roll': 0.0, 'pitch': 0.0, 'yaw': 0.0}
        self.quaternion = {'w': 1.0, 'x': 0.0, 'y': 0.0, 'z': 0.0}  # 四元数变量
        
        # 创建GUI界面
        self.create_widgets()
        
        # 初始化发布器
        self.setup_publisher()
        
        # 初始化四元数显示
        self.root.after(100, self.update_quaternion)  # 延迟100ms确保GUI完全创建
        
    def create_widgets(self):
        """创建GUI组件"""
        # 主框架
        main_frame = ttk.Frame(self.root, padding="10")
        main_frame.grid(row=0, column=0, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 配置行列权重
        self.root.columnconfigure(0, weight=1)
        self.root.rowconfigure(0, weight=1)
        main_frame.columnconfigure(1, weight=1)
        
        # 标题
        title_label = ttk.Label(main_frame, text="PoseStamped消息发布器", 
                               font=("Arial", 16, "bold"))
        title_label.grid(row=0, column=0, columnspan=3, pady=(0, 20))
        
        # 话题配置区域
        self.create_topic_config(main_frame, row=1)
        
        # 位置控制区域
        self.create_position_controls(main_frame, row=2)
        
        # 姿态控制区域
        self.create_orientation_controls(main_frame, row=3)
        
        # 四元数显示区域
        self.create_quaternion_display(main_frame, row=4)
        
        # 发布控制区域
        self.create_publish_controls(main_frame, row=5)
        
        # 状态显示区域
        self.create_status_display(main_frame, row=6)
        
    def create_topic_config(self, parent, row):
        """创建话题配置区域"""
        config_frame = ttk.LabelFrame(parent, text="话题配置", padding="10")
        config_frame.grid(row=row, column=0, columnspan=3, sticky=(tk.W, tk.E), pady=(0, 10))
        config_frame.columnconfigure(1, weight=1)
        
        # 话题名称
        ttk.Label(config_frame, text="话题名称:").grid(row=0, column=0, sticky=tk.W, padx=(0, 10))
        self.topic_var = tk.StringVar(value=self.topic_name)
        topic_entry = ttk.Entry(config_frame, textvariable=self.topic_var, width=40)
        topic_entry.grid(row=0, column=1, sticky=(tk.W, tk.E), padx=(0, 10))
        topic_entry.bind('<KeyRelease>', self.on_topic_change)
        
        # 发布频率
        ttk.Label(config_frame, text="发布频率(Hz):").grid(row=0, column=2, sticky=tk.W, padx=(10, 10))
        self.rate_var = tk.DoubleVar(value=self.publish_rate)
        rate_spinbox = ttk.Spinbox(config_frame, from_=0.1, to=50.0, increment=0.1, 
                                  textvariable=self.rate_var, width=10)
        rate_spinbox.grid(row=0, column=3, sticky=tk.W)
        rate_spinbox.bind('<KeyRelease>', self.on_rate_change)
        
    def create_position_controls(self, parent, row):
        """创建位置控制区域"""
        pos_frame = ttk.LabelFrame(parent, text="位置控制 (米)", padding="10")
        pos_frame.grid(row=row, column=0, columnspan=3, sticky=(tk.W, tk.E), pady=(0, 10))
        pos_frame.columnconfigure(1, weight=1)
        
        # X轴控制
        ttk.Label(pos_frame, text="X轴:").grid(row=0, column=0, sticky=tk.W, padx=(0, 10))
        self.x_var = tk.DoubleVar(value=self.position['x'])
        self.x_scale = ttk.Scale(pos_frame, from_=-10.0, to=10.0, orient=tk.HORIZONTAL,
                                variable=self.x_var, command=self.on_position_change)
        self.x_scale.grid(row=0, column=1, sticky=(tk.W, tk.E), padx=(0, 10))
        self.x_label = ttk.Label(pos_frame, text=f"{self.position['x']:.2f}")
        self.x_label.grid(row=0, column=2, sticky=tk.W)
        
        # Y轴控制
        ttk.Label(pos_frame, text="Y轴:").grid(row=1, column=0, sticky=tk.W, padx=(0, 10))
        self.y_var = tk.DoubleVar(value=self.position['y'])
        self.y_scale = ttk.Scale(pos_frame, from_=-10.0, to=10.0, orient=tk.HORIZONTAL,
                                variable=self.y_var, command=self.on_position_change)
        self.y_scale.grid(row=1, column=1, sticky=(tk.W, tk.E), padx=(0, 10))
        self.y_label = ttk.Label(pos_frame, text=f"{self.position['y']:.2f}")
        self.y_label.grid(row=1, column=2, sticky=tk.W)
        
        # Z轴控制
        ttk.Label(pos_frame, text="Z轴:").grid(row=2, column=0, sticky=tk.W, padx=(0, 10))
        self.z_var = tk.DoubleVar(value=self.position['z'])
        self.z_scale = ttk.Scale(pos_frame, from_=-3.0, to=3.0, orient=tk.HORIZONTAL,
                                variable=self.z_var, command=self.on_position_change)
        self.z_scale.grid(row=2, column=1, sticky=(tk.W, tk.E), padx=(0, 10))
        self.z_label = ttk.Label(pos_frame, text=f"{self.position['z']:.2f}")
        self.z_label.grid(row=2, column=2, sticky=tk.W)
        
    def create_orientation_controls(self, parent, row):
        """创建姿态控制区域"""
        ori_frame = ttk.LabelFrame(parent, text="姿态控制 (度)", padding="10")
        ori_frame.grid(row=row, column=0, columnspan=3, sticky=(tk.W, tk.E), pady=(0, 10))
        ori_frame.columnconfigure(1, weight=1)
        
        # Roll轴控制
        ttk.Label(ori_frame, text="Roll:").grid(row=0, column=0, sticky=tk.W, padx=(0, 10))
        self.roll_var = tk.DoubleVar(value=self.orientation['roll'])
        self.roll_scale = ttk.Scale(ori_frame, from_=-180.0, to=180.0, orient=tk.HORIZONTAL,
                                   variable=self.roll_var, command=self.on_orientation_change)
        self.roll_scale.grid(row=0, column=1, sticky=(tk.W, tk.E), padx=(0, 10))
        self.roll_label = ttk.Label(ori_frame, text=f"{self.orientation['roll']:.1f}°")
        self.roll_label.grid(row=0, column=2, sticky=tk.W)
        
        # Pitch轴控制
        ttk.Label(ori_frame, text="Pitch:").grid(row=1, column=0, sticky=tk.W, padx=(0, 10))
        self.pitch_var = tk.DoubleVar(value=self.orientation['pitch'])
        self.pitch_scale = ttk.Scale(ori_frame, from_=-180.0, to=180.0, orient=tk.HORIZONTAL,
                                    variable=self.pitch_var, command=self.on_orientation_change)
        self.pitch_scale.grid(row=1, column=1, sticky=(tk.W, tk.E), padx=(0, 10))
        self.pitch_label = ttk.Label(ori_frame, text=f"{self.orientation['pitch']:.1f}°")
        self.pitch_label.grid(row=1, column=2, sticky=tk.W)
        
        # Yaw轴控制
        ttk.Label(ori_frame, text="Yaw:").grid(row=2, column=0, sticky=tk.W, padx=(0, 10))
        self.yaw_var = tk.DoubleVar(value=self.orientation['yaw'])
        self.yaw_scale = ttk.Scale(ori_frame, from_=-180.0, to=180.0, orient=tk.HORIZONTAL,
                                  variable=self.yaw_var, command=self.on_orientation_change)
        self.yaw_scale.grid(row=2, column=1, sticky=(tk.W, tk.E), padx=(0, 10))
        self.yaw_label = ttk.Label(ori_frame, text=f"{self.orientation['yaw']:.1f}°")
        self.yaw_label.grid(row=2, column=2, sticky=tk.W)
        
    def create_quaternion_display(self, parent, row):
        """创建四元数显示区域"""
        quat_frame = ttk.LabelFrame(parent, text="四元数显示 (WXYZ)", padding="10")
        quat_frame.grid(row=row, column=0, columnspan=3, sticky=(tk.W, tk.E), pady=(0, 10))
        quat_frame.columnconfigure(1, weight=1)
        
        # W分量显示
        ttk.Label(quat_frame, text="W:").grid(row=0, column=0, sticky=tk.W, padx=(0, 10))
        self.quat_w_label = ttk.Label(quat_frame, text=f"{self.quaternion['w']:.4f}", 
                                     font=("Courier", 10), foreground="blue")
        self.quat_w_label.grid(row=0, column=1, sticky=tk.W, padx=(0, 20))
        
        # X分量显示
        ttk.Label(quat_frame, text="X:").grid(row=0, column=2, sticky=tk.W, padx=(0, 10))
        self.quat_x_label = ttk.Label(quat_frame, text=f"{self.quaternion['x']:.4f}", 
                                     font=("Courier", 10), foreground="red")
        self.quat_x_label.grid(row=0, column=3, sticky=tk.W, padx=(0, 20))
        
        # Y分量显示
        ttk.Label(quat_frame, text="Y:").grid(row=1, column=0, sticky=tk.W, padx=(0, 10))
        self.quat_y_label = ttk.Label(quat_frame, text=f"{self.quaternion['y']:.4f}", 
                                     font=("Courier", 10), foreground="green")
        self.quat_y_label.grid(row=1, column=1, sticky=tk.W, padx=(0, 20))
        
        # Z分量显示
        ttk.Label(quat_frame, text="Z:").grid(row=1, column=2, sticky=tk.W, padx=(0, 10))
        self.quat_z_label = ttk.Label(quat_frame, text=f"{self.quaternion['z']:.4f}", 
                                     font=("Courier", 10), foreground="purple")
        self.quat_z_label.grid(row=1, column=3, sticky=tk.W, padx=(0, 20))
        
        # 四元数模长显示
        quat_norm = (self.quaternion['w']**2 + self.quaternion['x']**2 + 
                    self.quaternion['y']**2 + self.quaternion['z']**2)**0.5
        ttk.Label(quat_frame, text="模长:").grid(row=2, column=0, sticky=tk.W, padx=(0, 10))
        self.quat_norm_label = ttk.Label(quat_frame, text=f"{quat_norm:.4f}", 
                                        font=("Courier", 10), foreground="black")
        self.quat_norm_label.grid(row=2, column=1, sticky=tk.W)
        
        # 添加说明文字
        info_label = ttk.Label(quat_frame, text="(四元数实时根据欧拉角计算)", 
                              font=("Arial", 8), foreground="gray")
        info_label.grid(row=2, column=2, columnspan=2, sticky=tk.W, padx=(20, 0))
        
    def create_publish_controls(self, parent, row):
        """创建发布控制区域"""
        control_frame = ttk.LabelFrame(parent, text="发布控制", padding="10")
        control_frame.grid(row=row, column=0, columnspan=3, sticky=(tk.W, tk.E), pady=(0, 10))
        
        # 开始/停止发布按钮
        self.publish_button = ttk.Button(control_frame, text="开始发布", 
                                        command=self.toggle_publishing)
        self.publish_button.grid(row=0, column=0, padx=(0, 10))
        
        # 发送单次消息按钮
        single_button = ttk.Button(control_frame, text="发送单次消息", 
                                  command=self.publish_single)
        single_button.grid(row=0, column=1, padx=(0, 10))
        
        # 重置位置按钮
        reset_button = ttk.Button(control_frame, text="重置位置", 
                                 command=self.reset_pose)
        reset_button.grid(row=0, column=2, padx=(0, 10))
        
        # 快速预设按钮
        preset_frame = ttk.Frame(control_frame)
        preset_frame.grid(row=1, column=0, columnspan=3, pady=(10, 0))
        
        ttk.Label(preset_frame, text="快速预设:").grid(row=0, column=0, padx=(0, 10))
        
        presets = [
            ("起飞位置", {"x": 0, "y": 0, "z": 1.5, "yaw": 0}),
            ("地面位置", {"x": 0, "y": 0, "z": 0, "yaw": 0}),
            ("前进", {"x": 3, "y": 0, "z": 1.5, "yaw": 0}),
            ("右转", {"x": 3, "y": 3, "z": 1.5, "yaw": 90}),
            ("返回", {"x": 0, "y": 0, "z": 0, "yaw": 0})
        ]
        
        for i, (name, pose) in enumerate(presets):
            btn = ttk.Button(preset_frame, text=name, 
                           command=lambda p=pose: self.set_preset_pose(p))
            btn.grid(row=0, column=i+1, padx=2)
        
    def create_status_display(self, parent, row):
        """创建状态显示区域"""
        status_frame = ttk.LabelFrame(parent, text="状态信息", padding="10")
        status_frame.grid(row=row, column=0, columnspan=3, sticky=(tk.W, tk.E, tk.N, tk.S), pady=(0, 10))
        status_frame.columnconfigure(0, weight=1)
        parent.rowconfigure(row, weight=1)
        
        # 状态文本显示
        self.status_text = tk.Text(status_frame, height=8, width=80, wrap=tk.WORD,
                                  state=tk.DISABLED, bg="#f0f0f0")
        self.status_text.grid(row=0, column=0, sticky=(tk.W, tk.E, tk.N, tk.S))
        
        # 滚动条
        scrollbar = ttk.Scrollbar(status_frame, orient=tk.VERTICAL, command=self.status_text.yview)
        scrollbar.grid(row=0, column=1, sticky=(tk.N, tk.S))
        self.status_text.config(yscrollcommand=scrollbar.set)
        
        # 初始状态信息
        self.update_status("程序启动完成")
        self.update_status(f"默认话题: {self.topic_name}")
        self.update_status(f"发布频率: {self.publish_rate} Hz")
        
    def setup_publisher(self):
        """设置ROS发布器"""
        try:
            self.publisher = rospy.Publisher(self.topic_name, PoseStamped, queue_size=10)
            self.update_status(f"发布器创建成功: {self.topic_name}")
        except Exception as e:
            self.update_status(f"发布器创建失败: {str(e)}")
            messagebox.showerror("错误", f"发布器创建失败: {str(e)}")
            
    def on_topic_change(self, event=None):
        """话题名称变化回调"""
        new_topic = self.topic_var.get().strip()
        if new_topic and new_topic != self.topic_name:
            self.topic_name = new_topic
            self.setup_publisher()
            
    def on_rate_change(self, event=None):
        """发布频率变化回调"""
        try:
            new_rate = self.rate_var.get()
            if new_rate > 0:
                self.publish_rate = new_rate
                self.update_status(f"发布频率更新: {self.publish_rate} Hz")
        except:
            pass
            
    def on_position_change(self, value=None):
        """位置变化回调"""
        self.position['x'] = self.x_var.get()
        self.position['y'] = self.y_var.get()
        self.position['z'] = self.z_var.get()
        
        # 更新标签显示
        self.x_label.config(text=f"{self.position['x']:.2f}")
        self.y_label.config(text=f"{self.position['y']:.2f}")
        self.z_label.config(text=f"{self.position['z']:.2f}")
        
    def on_orientation_change(self, value=None):
        """姿态变化回调"""
        self.orientation['roll'] = self.roll_var.get()
        self.orientation['pitch'] = self.pitch_var.get()
        self.orientation['yaw'] = self.yaw_var.get()
        
        # 更新标签显示
        self.roll_label.config(text=f"{self.orientation['roll']:.1f}°")
        self.pitch_label.config(text=f"{self.orientation['pitch']:.1f}°")
        self.yaw_label.config(text=f"{self.orientation['yaw']:.1f}°")
        
        # 更新四元数
        self.update_quaternion()
        
    def update_quaternion(self):
        """根据欧拉角更新四元数并显示"""
        import math
        
        # 转换为弧度
        roll = math.radians(self.orientation['roll'])
        pitch = math.radians(self.orientation['pitch']) 
        yaw = math.radians(self.orientation['yaw'])
        
        # 计算四元数
        quaternion = quaternion_from_euler(roll, pitch, yaw)
        self.quaternion['x'] = quaternion[0]
        self.quaternion['y'] = quaternion[1]
        self.quaternion['z'] = quaternion[2]
        self.quaternion['w'] = quaternion[3]
        
        # 更新四元数显示标签
        if hasattr(self, 'quat_w_label'):
            self.quat_w_label.config(text=f"{self.quaternion['w']:.4f}")
            self.quat_x_label.config(text=f"{self.quaternion['x']:.4f}")
            self.quat_y_label.config(text=f"{self.quaternion['y']:.4f}")
            self.quat_z_label.config(text=f"{self.quaternion['z']:.4f}")
            
            # 更新模长
            quat_norm = (self.quaternion['w']**2 + self.quaternion['x']**2 + 
                        self.quaternion['y']**2 + self.quaternion['z']**2)**0.5
            self.quat_norm_label.config(text=f"{quat_norm:.4f}")
        
    def create_pose_message(self):
        """创建PoseStamped消息"""
        msg = PoseStamped()
        
        # 设置header
        msg.header.stamp = rospy.Time.now()
        msg.header.frame_id = "map"
        
        # 设置位置
        msg.pose.position.x = self.position['x']
        msg.pose.position.y = self.position['y']
        msg.pose.position.z = self.position['z']
        
        # 设置姿态 (从欧拉角转换为四元数)
        import math
        roll = math.radians(self.orientation['roll'])
        pitch = math.radians(self.orientation['pitch'])
        yaw = math.radians(self.orientation['yaw'])
        
        quaternion = quaternion_from_euler(roll, pitch, yaw)
        msg.pose.orientation.x = quaternion[0]
        msg.pose.orientation.y = quaternion[1]
        msg.pose.orientation.z = quaternion[2]
        msg.pose.orientation.w = quaternion[3]
        
        return msg
        
    def publish_single(self):
        """发布单次消息"""
        if self.publisher is None:
            messagebox.showerror("错误", "发布器未初始化")
            return
            
        try:
            msg = self.create_pose_message()
            self.publisher.publish(msg)
            self.update_status(f"发送单次消息: 位置({self.position['x']:.2f}, {self.position['y']:.2f}, {self.position['z']:.2f}), "
                             f"欧拉角({self.orientation['roll']:.1f}°, {self.orientation['pitch']:.1f}°, {self.orientation['yaw']:.1f}°), "
                             f"四元数(w:{self.quaternion['w']:.3f}, x:{self.quaternion['x']:.3f}, y:{self.quaternion['y']:.3f}, z:{self.quaternion['z']:.3f})")
        except Exception as e:
            self.update_status(f"发送消息失败: {str(e)}")
            messagebox.showerror("错误", f"发送消息失败: {str(e)}")
            
    def toggle_publishing(self):
        """切换连续发布状态"""
        if not self.is_publishing:
            self.start_publishing()
        else:
            self.stop_publishing()
            
    def start_publishing(self):
        """开始连续发布"""
        if self.publisher is None:
            messagebox.showerror("错误", "发布器未初始化")
            return
            
        self.is_publishing = True
        self.publish_button.config(text="停止发布", style="Accent.TButton")
        
        # 启动发布线程
        self.publish_thread = threading.Thread(target=self.publish_loop, daemon=True)
        self.publish_thread.start()
        
        self.update_status(f"开始连续发布到话题: {self.topic_name}")
        
    def stop_publishing(self):
        """停止连续发布"""
        self.is_publishing = False
        self.publish_button.config(text="开始发布", style="")
        self.update_status("停止连续发布")
        
    def publish_loop(self):
        """发布循环线程"""
        rate = rospy.Rate(self.publish_rate)
        
        while self.is_publishing and not rospy.is_shutdown():
            try:
                msg = self.create_pose_message()
                self.publisher.publish(msg)
                rate.sleep()
            except Exception as e:
                self.update_status(f"发布循环错误: {str(e)}")
                break
                
    def set_preset_pose(self, pose):
        """设置预设位置"""
        # 更新滑条值
        self.x_var.set(pose.get('x', 0))
        self.y_var.set(pose.get('y', 0))
        self.z_var.set(pose.get('z', 0))  # 默认z值改为0
        self.yaw_var.set(pose.get('yaw', 0))
        
        # 如果预设中有roll和pitch，也设置它们
        if 'roll' in pose:
            self.roll_var.set(pose['roll'])
        if 'pitch' in pose:
            self.pitch_var.set(pose['pitch'])
        
        # 触发更新
        self.on_position_change()
        self.on_orientation_change()
        
        self.update_status(f"设置预设位置: {pose}")
        
    def reset_pose(self):
        """重置位置和姿态"""
        # 重置所有滑条
        self.x_var.set(0.0)
        self.y_var.set(0.0)
        self.z_var.set(0.0)  # z轴重置为0
        self.roll_var.set(0.0)
        self.pitch_var.set(0.0)
        self.yaw_var.set(0.0)
        
        # 触发更新
        self.on_position_change()
        self.on_orientation_change()
        
        self.update_status("位置和姿态已重置到原点")
        
    def update_status(self, message):
        """更新状态显示"""
        timestamp = time.strftime("%H:%M:%S")
        formatted_message = f"[{timestamp}] {message}\n"
        
        self.status_text.config(state=tk.NORMAL)
        self.status_text.insert(tk.END, formatted_message)
        self.status_text.see(tk.END)
        self.status_text.config(state=tk.DISABLED)
        
    def on_closing(self):
        """程序关闭时的清理"""
        if self.is_publishing:
            self.stop_publishing()
        self.root.destroy()

def main():
    """主函数"""
    # 创建主窗口
    root = tk.Tk()
    
    # 创建应用实例
    app = PosePublisherGUI(root)
    
    # 设置关闭事件
    root.protocol("WM_DELETE_WINDOW", app.on_closing)
    
    # 运行GUI主循环
    try:
        root.mainloop()
    except KeyboardInterrupt:
        print("\n程序被用户中断")
    except Exception as e:
        print(f"程序运行错误: {e}")
    finally:
        if not rospy.is_shutdown():
            rospy.signal_shutdown("GUI关闭")

if __name__ == "__main__":
    main()
